#include "FOC.h"
#include "AS5600.h" 
#include <math.h>
#include "pid.h"
#include "LOWPASS_FILTER.h"

int PP=11;	//极对数
int DIR=-1;	//无刷电机纠偏旋转方向
int voltage_power_supply=12;//供电电压
float phase_resistance=5.75;//单相电阻


//float zero_electric_angle=3.724124;//2804_1
//float zero_electric_angle=3.724124;//2804_2
float zero_electric_angle=4.4219;//4108


float voltage_limit = 10;
float shaft_angle=0,open_loop_timestamp=0;//转子角度与开环控制时间
float Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0,dc_a=0,dc_b=0,dc_c=0;
float i_alpha,i_beta=0,ia=0,ib=0,ic=0,ira=0,irb=0,irc=0,iq,id;

/*******************************************
电角度求解：

备注：     
	1.     电角度      =   机械角度  *  极对数 - 偏差电角度
	2._electricalAngle = shaft_angle * pole_pairs - zero_electrical_angle
*******************************************/
float _shaft_angle(void)
{
	return _normalizeAngle(get_Angle_2PI()-zero_electric_angle/PP);
}

float _electricalAngle(void)
{
	return _normalizeAngle((float)(DIR * PP) * get_Angle_2PI()-zero_electric_angle);
}
/*******************************************
角度归一化处理:输出角度为0~2Pi
*******************************************/
float _normalizeAngle(float angle)
{
	float a = fmod(angle,_2PI);
	return a >= 0 ? a : (a+ _2PI);
	//三目运算符。格式：condition ? expr1 : expr2
	//其中,condition式要求值的条件表达式,如果条件成立,则返回 expr1 的值。可以将三目运算符看作if-else的简化形式
}


/*******************************************
开环力矩控制设置相电压:
①给定目标Uq(定子合成磁场垂直于转子磁场的的磁通分量)Ud(定子合成磁场平行于转子磁场的的磁通分量)
	数值大小在-
②给定当前转子位置（电角度）
③基于上述三个量通过帕克逆变换得到"定子2坐标系"下的Uα与Uβ
④基于帕克逆变换得到的Uα与Uβ通过克拉克逆变换得到定子3坐标系下的U_A\U_B\U_C
⑤最后根据U_a、U_b、U_c计算出每相半桥输出的占空比，并且赋值

*******************************************/
void setPhaseVoltage(float Uq,float Ud, float angle_el)
{
	//对Uq进行限幅
	Uq = _constrain(Uq,-voltage_power_supply/3,voltage_power_supply/3);
	
	//帕克逆变换
	Ualpha = -Uq*sin(angle_el);
	Ubeta  = Uq*cos(angle_el); 
	//克拉克逆变换
	Ua = Ualpha + voltage_power_supply/2;
	Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2;
	Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2;
	
	
	
	dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f );
	dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f );
	dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f );
	
	SetPWM_A(dc_a);
    SetPWM_B(dc_b);
	SetPWM_C(dc_c);
}


//克拉克等幅值,0°时ia=ialpha
void Clarke_Transf(float *iphase,float I_alpha,float I_beta)
{
	I_alpha = iphase[0];
	I_beta = (iphase[1]-iphase[2])/sqrt(3);
}


void Park_Transf(float I_alpha,float I_beta,float i_q,float i_d,float angle_el)
{
	i_q=I_alpha*cos(angle_el)+I_beta*sin(angle_el);
	i_d=I_beta*cos(angle_el)-I_alpha*sin(angle_el);
}
/*******************************************
闭环力矩控制:
①将获取到的三相电流ia\ib\ic输入并通过克拉克-帕克变换转换为iq
②
③给定当前转子位置（电角度）
④基于上述三个量通过帕克逆变换得到"定子2坐标系"下的Uα与Uβ
⑤基于帕克逆变换得到的Uα与Uβ通过克拉克逆变换得到定子3坐标系下的U_A\U_B\U_C
最后根据U_a、U_b、U_c计算出每相半桥输出的占空比，并且赋值

*******************************************/
float setPhaseCurrent(float Target_Iq,float Target_Id, float angle_el,float *current_value,float *vofa)
{
	
	 

	 
	i_alpha = current_value[0];
	i_beta = _2_SQRT3*current_value[1]+current_value[0]*_1_SQRT3;
	
//	id=i_beta*sin(angle_el)+i_alpha*cos(angle_el);
	iq=i_beta*cos(angle_el)-i_alpha*sin(angle_el);
	
	iq=LowPass_Filter_current(iq);
	
	vofa[0] = current_value[0];
	vofa[1]=iq;
	
	return currentcloseloop(Target_Iq, iq);
}
//上电自动矫正0电角度
float FOC_init()
{
	setPhaseVoltage(3.5, 0,_3PI_2);
	
	HAL_Delay(3000);
	zero_electric_angle=_electricalAngle();
	setPhaseVoltage(0, 0,_3PI_2);
	
	return	zero_electric_angle;
}


//输入目标速度target_velocity与控制周期ts(s)进行闭环速度控制
//***************增量式PID控制函数************************
//target_velocity：目标速度 (rpm)
//Real_velocity：实际速度(rpm)
//返回值：PID计算后输出值
float velocitycloseloop(float target_velocity,float feedback_velocity)
{	
	g_speed_pid.SetPoint = target_velocity;
//	shaft_angle = _normalizeAngle(shaft_angle + target_velocity*ts);

	return increment_pid_ctrl(&g_speed_pid,feedback_velocity,0,0);
	
}

//输入目标速度target_velocity与控制周期ts(s)进行闭环速度控制
//***************增量式PID控制函数************************
//target_velocity：目标机械角度 (°)
//Real_velocity：实际机械角度 (°)
//返回值：PID计算后输出值
float positioncloseloop(float target_position,float feedback_position)
{	
	
	g_location_pid.SetPoint = target_position;
	//需当偏差值为90度时输出最大电压6V，则p为0.06
	return location_pid_ctrl(&g_location_pid,feedback_position,0,0);
	
}

void get_current_test(float *current_value)
{
	current_value[0]=3.208138f-HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_1)*0.00161132f;
	current_value[1]=3.211361f-HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_2)*0.00161132f;
	current_value[2]=3.238753f-HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_3)*0.00161132f;
}



//输入目标速度target_velocity与控制周期ts(s)进行闭环速度控制
//***************增量式PID控制函数************************
//target_current：目标机械角度 (°)
//Real_velocity：实际机械角度 (°)
//返回值：PID计算后输出值
float currentcloseloop(float target_current,float feedback_current)
{	
	
	g_current_pid.SetPoint = target_current;
	//需当偏差值为0.5A时输出最大电压6V，则p为10
	return location_pid_ctrl(&g_current_pid,feedback_current,0,0);
}


